﻿using System;
using OpenTK;

namespace AnimatingHair.Auxiliary
{
    class Geometry
    {
        public class Sphere
        {
            public Vector3d Center { get; set; }
            public double Radius { get; set; }
        }

        public class Cylinder
        {
            public Vector3d Endpoint1 { get; set; }
            public Vector3d Endpoint2 { get; set; }
            public double Radius { get; set; }

            public double LengthSqared, RadiusSquared;

            public Cylinder( Vector3d endpoint1, Vector3d endpoint2, double radius )
            {
                Endpoint1 = endpoint1;
                Endpoint2 = endpoint2;
                Radius = radius;
                RadiusSquared = radius * radius;
                LengthSqared = (endpoint2 - endpoint1).LengthSquared;
            }
        }

        /// <summary>
        /// This function tests if the 3D point 'point' lies within an arbitrarily oriented cylinder 'cylinder'.
        /// </summary>
        /// <returns>
        /// -1.0 if point is outside the cylinder;
        /// distance squared from cylinder axis if point is inside.
        /// </returns>
        public static double PointCylinderTest( Cylinder cylinder, Vector3d point )
        {
            double dx, dy, dz;	// vector d  from line segment point 1 to point 2
            double pdx, pdy, pdz;	// vector pd from point 1 to test point
            double dot, dsq;

            dx = cylinder.Endpoint2.X - cylinder.Endpoint1.X;	// translate so cylinder.Endpoint1 is origin.  Make vector from
            dy = cylinder.Endpoint2.Y - cylinder.Endpoint1.Y;   // cylinder.Endpoint1 to cylinder.Endpoint2.  Need for this is easily eliminated
            dz = cylinder.Endpoint2.Z - cylinder.Endpoint1.Z;

            pdx = point.X - cylinder.Endpoint1.X;		// vector from cylinder.Endpoint1 to test point.
            pdy = point.Y - cylinder.Endpoint1.Y;
            pdz = point.Z - cylinder.Endpoint1.Z;

            // Dot the d and pd vectors to see if point lies behind the 
            // cylinder cap at cylinder.Endpoint1.X, cylinder.Endpoint1.Y, cylinder.Endpoint1.Z

            dot = pdx * dx + pdy * dy + pdz * dz;

            // If dot is less than zero the point is behind the cylinder.Endpoint1 cap.
            // If greater than the cylinder axis line segment length squared
            // then the point is outside the other end cap at cylinder.Endpoint2.

            if ( dot < 0.0 || dot > cylinder.LengthSqared )
            {
                return (-1.0);
            }
            else
            {
                // Point lies within the parallel caps, so find
                // distance squared from point to line, using the fact that sin^2 + cos^2 = 1
                // the dot = cos() * |d||pd|, and cross*cross = sin^2 * |d|^2 * |pd|^2
                // Carefull: '*' means mult for scalars and dotproduct for vectors
                // In short, where dist is pt distance to cyl axis: 
                // dist = sin( pd to d ) * |pd|
                // distsq = dsq = (1 - cos^2( pd to d)) * |pd|^2
                // dsq = ( 1 - (pd * d)^2 / (|pd|^2 * |d|^2) ) * |pd|^2
                // dsq = pd * pd - dot * dot / lengthsq
                //  where lengthsq is d*d or |d|^2 that is passed into this function 

                // distance squared to the cylinder axis:

                dsq = (pdx * pdx + pdy * pdy + pdz * pdz) - dot * dot / cylinder.LengthSqared;

                if ( dsq > cylinder.RadiusSquared )
                {
                    return -1.0;
                }
                else
                {
                    return Math.Sqrt( dsq );		// return distance squared to axis
                }
            }
        }

        /// <summary>
        /// Testuje, ci bod lezi v prieniku dvoch polrovin, ktore smeruju od okrajov valca k sebe.
        /// </summary>
        public static bool PointInInfiniteRadiusCylinder( Cylinder cylinder, Vector3d point )
        {
            double dx, dy, dz;	// vector d  from line segment point 1 to point 2
            double pdx, pdy, pdz;	// vector pd from point 1 to test point
            double dot;

            dx = cylinder.Endpoint2.X - cylinder.Endpoint1.X;	// translate so cylinder.Endpoint1 is origin.  Make vector from
            dy = cylinder.Endpoint2.Y - cylinder.Endpoint1.Y;   // cylinder.Endpoint1 to cylinder.Endpoint2.  Need for this is easily eliminated
            dz = cylinder.Endpoint2.Z - cylinder.Endpoint1.Z;

            pdx = point.X - cylinder.Endpoint1.X;		// vector from cylinder.Endpoint1 to test point.
            pdy = point.Y - cylinder.Endpoint1.Y;
            pdz = point.Z - cylinder.Endpoint1.Z;

            // Dot the d and pd vectors to see if point lies behind the 
            // cylinder cap at cylinder.Endpoint1.X, cylinder.Endpoint1.Y, cylinder.Endpoint1.Z

            dot = pdx * dx + pdy * dy + pdz * dz;

            // If dot is less than zero the point is behind the cylinder.Endpoint1 cap.
            // If greater than the cylinder axis line segment length squared
            // then the point is outside the other end cap at cylinder.Endpoint2.

            if ( dot < 0.0 || dot > cylinder.LengthSqared )
                return false;
            else
                return true;
        }

        public static Vector3d LineToPointNormal( Vector3d lineStart, Vector3d lineEnd, Vector3d point )
        {
            Vector3d v1 = lineEnd - lineStart;
            Vector3d v2 = point - lineStart;

            double dotV1V2 = Vector3d.Dot( v1, v2 );
            double dotV1V1 = Vector3d.Dot( v1, v1 );

            if ( dotV1V1 < 0.00000001 )
                return new Vector3d( 0, 0, 0 );

            double a = -dotV1V2 / dotV1V1;

            Vector3d kolmyPriemet = -a * v1;

            return (point - kolmyPriemet) - lineStart;
        }
    }
}
